package com.gunncs.actoriface;
import com.gunncs.actoriface.*;
public class DecisionActor2 implements Actor{
  double[][] histFL = new double[3][2];
  double[][] histFR = new double[3][2];
  double[][] histBL = new double[3][2];
  double[][] histBR = new double[3][2]; 

  int direction;
  boolean bumped;
  // the previous decision made
  Decision current;

  // the root of the decision tree
  Decision root;
  // TODO: set tolerances
  // amount of distance to go forward before stopping and looking around
  static final int DISTANCE_FORWARD = 30;
  
  static final int CLOSED=-99;
  // maximum distance to be considered a wall (and not a corridor)
  static final double WALL_DISTANCE = 20.;

  // time until servo stops
  static final int SLEEP_TIME = 1200;
  
  static final int BREAKTOLERANCE = 10;
  
  public DecisionActor2(){
    root=new Decision(Decision.NORTH, Decision.NORTH);
    current=root;
    root.select();
    bumped=false;
  }
  
  public String getMaze(){
    return "Example";
  } 
  
  public void act(Robot r) {
    boolean turning = (RangeFinderAnalysis.risingOrFalling(true, histFL) || RangeFinderAnalysis.risingOrFalling(true, histFR));
    if(turning || bumped) {
      
      // move out into the middle of the turn, then fire the servo over to point straight forward
    
      if(turning) enterHole(r);
      else 
        r.getServo().rotate(-45);
      // wait for the servo to rotate all the way around
      try{
       Thread.sleep(SLEEP_TIME);
      }catch(Exception e){
        
      }
      boolean makingTurns =true;
      while(makingTurns) {
        int[] holes = findHoles();
        for(int i=0; i<holes.length; i++){
          if(holes[i]!=CLOSED) {
            current.addChoice(new Decision(holes[i],direction));
          }
        }
         makingTurns = !(holes[1]==-1 && holes[2]==-1);
        Decision d = decide(); // where are we going now?
        if(d == null){
          undo(r);
        }else {
          doDecision(d, r);
        }
      
        exitHole(r);
      }
    }
  }


  public void doDecision(Decision d, Robot r) {
    if (d.getOptionDir() + 1 % 4 == d.getCameFrom()) {
      r.turnRight();
      r.waitAngle(90);
    } else if (d.getOptionDir() + 3 % 4 == d.getCameFrom()) {
      r.turnLeft();
      r.waitAngle(90);
    }
    
  }

  // complete the first half of the turn, rotating the servo 45 degrees
  public void enterHole(Robot r) {
    r.goForward();
    r.waitDistance(DISTANCE_FORWARD);
    r.getServo().rotate(-45);
  }
  
  // complete the second half of the turn
  public void exitHole(Robot r) {
    r.goForward();
    r.waitDistance(DISTANCE_FORWARD);
  }
  
  public void undo(Robot r) {
    current.fail();
    r.turnLeft();
    r.waitAngle(180);
  }
  
  public int[] findHoles(){
    int options[] = new int[3];
    for(int i=0; i<4; i++){
      options[i]=CLOSED;     
    }
    if(histFL[0][0] - WALL_DISTANCE > BREAKTOLERANCE){
       //THERES a straight hole
       options[0]=direction;
    }
    if(histFR[0][0] - WALL_DISTANCE > BREAKTOLERANCE){
       //THERES a RIGHT hole
       options[1]=RangeFinderAnalysis.rotateDirection(direction, true);
    }
    if(histBL[0][0] - WALL_DISTANCE > BREAKTOLERANCE){
       //THERES a LEFT hole
       options[2]=RangeFinderAnalysis.rotateDirection(direction,false);
    }
   
    return options;
  }

  public double[][] shift(double[][] arr) {
    arr[0] = arr[1];
    arr[1] = arr[2];
    return arr;
  }

  public void updateSensors(Sensor[] sensors) {
     
     bumped=false;
     if(sensors[0].getState()[0] == 1 && sensors[0].getState()[1] == 1)
       bumped=true;
     if (histFL[2][0] != 0) {
       histFL = shift(histFL);
     }
     if (histFR[2][0] != 0) {
       histFR = shift(histFR);
     }
     if (histBL[2][0] != 0) {
       histBL = shift(histBL);
     }
     if (histBR[2][0] != 0) {
       histBR = shift(histBR);
     }

     histFL[0] = sensors[4].getState();
     histFR[0] = sensors[1].getState();
     histBR[0] = sensors[2].getState();
     histBL[0] = sensors[3].getState();
  }
  public Decision decide(){
    for(int i=0; i<current.subsequent.size(); i++){
      if(!current.subsequent.get(i).getFail()){
        current.subsequent.get(i).select();
        return current.subsequent.get(i);
      }
    }
    return null;
  }


}
